﻿using System;
using System.Collections.Generic;
using System.Data.Linq;
using System.Text;

namespace GraphSpace
{
	class uniSearch<PQ> where PQ : IPQ<DijkstraKnoten>, new()
	{
		PQ PriorityStuct = new PQ ();

        public uniSearch(int C)
        {
            PriorityStuct.init(C);
        }
        public uniSearch()
        {

        }

        public double DoSearch(Graph<DijkstraKnoten, Kante> G, Knoten initialKnoten, Knoten endKnoten, out Knoten[] Path, bool GoalDirected)
		{
			
			for (int i = 0; i < G.V.Count; i++) {
         //       try
          //      {
                    PriorityStuct.insert((DijkstraKnoten)G.V[i]);
          //      }
//                catch (Exception e)
//                {
//                    Console.WriteLine("uniSearch::DoSearch : " + e.Message);
//                    Path = new Knoten[0];
//                    return 0.0;
//                }
			}
		//	try {
				((DijkstraKnoten)initialKnoten).distance = 0;
				while (!PriorityStuct.isEmpty ()) {
					
					Knoten aktKnoten = PriorityStuct.extractHead ();
					if (aktKnoten.Id == endKnoten.Id)
						break;
					((DijkstraKnoten)aktKnoten).besucht = true;
					for (int i = 0; i < G.E[aktKnoten.Id].Count; i++) {
						Knoten ZielKnoten = (Knoten)G.E[aktKnoten.Id][i].Vnach;
						if (!((DijkstraKnoten)ZielKnoten).besucht) {
							//Unterschied zwischen normal oder zielgerichtet
							if (GoalDirected) {
								// neue abstandsberechnung fuer AStar
								//int NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht;
								
								int AbstandX = ((DijkstraKnoten)endKnoten).x - ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).x;
								int AbstandY = ((DijkstraKnoten)endKnoten).y - ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).y;
								double NewDist = ((DijkstraKnoten)aktKnoten).distance + G.E[aktKnoten.Id][i].Gewicht + Math.Sqrt (AbstandX * AbstandX + AbstandY * AbstandY);
								if (NewDist < ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance) {
									((DijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
									PriorityStuct.decreaseKey (((DijkstraKnoten)ZielKnoten).getHeapPos (), NewDist);
									((DijkstraKnoten)ZielKnoten).realdist = ((DijkstraKnoten)aktKnoten).realdist + G.E[aktKnoten.Id][i].Gewicht;
								}
							} else {
								if (!((DijkstraKnoten)ZielKnoten).besucht) {
									double NewDist = ((DijkstraKnoten)aktKnoten).distance + (int)G.E[aktKnoten.Id][i].Gewicht;
									if (NewDist < ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance) {
										((DijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
										PriorityStuct.decreaseKey (((DijkstraKnoten)ZielKnoten).getHeapPos (), NewDist);
									}
								}
							}
						}
					}
					
				}
				
				if (GoalDirected) {
					DijkstraKnoten pred = ((DijkstraKnoten)endKnoten);
					List<DijkstraKnoten> path = new List<DijkstraKnoten> ();
					while (pred != null) {
						path.Add (pred);
						pred = (DijkstraKnoten)pred.predeseccor;
					}
					
					path.Reverse ();
					Path = path.ToArray ();
					return ((DijkstraKnoten)endKnoten).realdist;
				} else {
					DijkstraKnoten pred = (DijkstraKnoten)endKnoten;
					List<DijkstraKnoten> path = new List<DijkstraKnoten> ();
					while (pred != null) {
						path.Add (pred);
						pred = (DijkstraKnoten)pred.predeseccor;
					}
					
					path.Reverse ();
					Path = path.ToArray ();
					return ((DijkstraKnoten)endKnoten).distance;
				}
		//	} 
//			catch (Exception e) 
//			{
//				Console.WriteLine ("uniSearch::DoSearch : " + e.Message);	
//			}
			Path = new Knoten[0];
			return 0.0;
		}
	}
}

